// 标定结果比较分析程序
// 功能：比较不同标定方法的结果差异，分析原因

#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>

struct CalibrationResult {
    std::string method;
    cv::Mat camera_matrix;
    cv::Mat distortion_coeffs;
    double rms_error;
    int image_count;
    cv::Size image_size;
};

class CalibrationComparison {
private:
    std::vector<CalibrationResult> results_;
    std::string output_file_;

public:
    CalibrationComparison() : output_file_("calibration_comparison_analysis.txt") {
        ROS_INFO("Calibration Comparison Analysis initialized");
    }
    
    void loadCalibrationResults() {
        // 加载相机信息数据
        loadCameraInfoResult();
        
        // 加载ROS标定结果
        loadROSCalibrationResult();
        
        // 加载Autoware标定结果
        loadAutowareCalibrationResult();
        
        ROS_INFO("Loaded %zu calibration results", results_.size());
    }
    
    void loadCameraInfoResult() {
        // 这里应该从/camera/color/camera_info话题读取的数据
        // 为了演示，我们创建一个模拟结果
        CalibrationResult result;
        result.method = "Camera Info Topic";
        result.camera_matrix = (cv::Mat_<double>(3, 3) << 
            587.0, 0.0, 320.0,
            0.0, 587.0, 240.0,
            0.0, 0.0, 1.0);
        result.distortion_coeffs = (cv::Mat_<double>(1, 5) << 0.1, -0.2, 0.0, 0.0, 0.0);
        result.rms_error = 0.0; // 未知
        result.image_count = 1;
        result.image_size = cv::Size(640, 480);
        
        results_.push_back(result);
        ROS_INFO("Loaded Camera Info result");
    }
    
    void loadROSCalibrationResult() {
        CalibrationResult result;
        result.method = "ROS Official Calibration";
        
        // 尝试加载ROS标定结果文件
        cv::FileStorage fs("ros_camera_calibration_result.yaml", cv::FileStorage::READ);
        if (fs.isOpened()) {
            fs["camera_matrix"] >> result.camera_matrix;
            fs["distortion_coefficients"] >> result.distortion_coeffs;
            fs["rms_reprojection_error"] >> result.rms_error;
            fs["number_of_images"] >> result.image_count;
            fs["image_width"] >> result.image_size.width;
            fs["image_height"] >> result.image_size.height;
            fs.release();
            
            results_.push_back(result);
            ROS_INFO("Loaded ROS calibration result");
        } else {
            ROS_WARN("ROS calibration result file not found");
        }
    }
    
    void loadAutowareCalibrationResult() {
        CalibrationResult result;
        result.method = "Autoware Calibration";
        
        // 尝试加载Autoware标定结果文件
        cv::FileStorage fs("autoware_camera_calibration_result.yaml", cv::FileStorage::READ);
        if (fs.isOpened()) {
            fs["camera_matrix"] >> result.camera_matrix;
            fs["distortion_coefficients"] >> result.distortion_coeffs;
            fs["rms_reprojection_error"] >> result.rms_error;
            fs["number_of_images"] >> result.image_count;
            fs["image_width"] >> result.image_size.width;
            fs["image_height"] >> result.image_size.height;
            fs.release();
            
            results_.push_back(result);
            ROS_INFO("Loaded Autoware calibration result");
        } else {
            ROS_WARN("Autoware calibration result file not found");
        }
    }
    
    void performComparison() {
        if (results_.size() < 2) {
            ROS_WARN("Need at least 2 calibration results for comparison");
            return;
        }
        
        ROS_INFO("=== Calibration Results Comparison ===");
        
        // 比较相机内参矩阵
        compareCameraMatrices();
        
        // 比较畸变系数
        compareDistortionCoefficients();
        
        // 比较重投影误差
        compareReprojectionErrors();
        
        // 分析差异原因
        analyzeDifferences();
        
        // 保存比较结果
        saveComparisonResults();
    }
    
    void compareCameraMatrices() {
        ROS_INFO("\n=== Camera Matrix Comparison ===");
        
        for (size_t i = 0; i < results_.size(); i++) {
            ROS_INFO("\n%s:", results_[i].method.c_str());
            ROS_INFO("  fx: %.6f, fy: %.6f", 
                    results_[i].camera_matrix.at<double>(0, 0),
                    results_[i].camera_matrix.at<double>(1, 1));
            ROS_INFO("  cx: %.6f, cy: %.6f", 
                    results_[i].camera_matrix.at<double>(0, 2),
                    results_[i].camera_matrix.at<double>(1, 2));
        }
        
        // 计算差异
        if (results_.size() >= 2) {
            double fx_diff = std::abs(results_[0].camera_matrix.at<double>(0, 0) - 
                                    results_[1].camera_matrix.at<double>(0, 0));
            double fy_diff = std::abs(results_[0].camera_matrix.at<double>(1, 1) - 
                                    results_[1].camera_matrix.at<double>(1, 1));
            
            ROS_INFO("\nDifferences:");
            ROS_INFO("  fx difference: %.6f pixels", fx_diff);
            ROS_INFO("  fy difference: %.6f pixels", fy_diff);
        }
    }
    
    void compareDistortionCoefficients() {
        ROS_INFO("\n=== Distortion Coefficients Comparison ===");
        
        for (size_t i = 0; i < results_.size(); i++) {
            ROS_INFO("\n%s:", results_[i].method.c_str());
            for (int j = 0; j < results_[i].distortion_coeffs.cols; j++) {
                ROS_INFO("  k%d: %.6f", j, results_[i].distortion_coeffs.at<double>(j));
            }
        }
    }
    
    void compareReprojectionErrors() {
        ROS_INFO("\n=== Reprojection Error Comparison ===");
        
        for (size_t i = 0; i < results_.size(); i++) {
            ROS_INFO("%s: %.6f pixels (using %d images)", 
                    results_[i].method.c_str(), 
                    results_[i].rms_error,
                    results_[i].image_count);
        }
    }
    
    void analyzeDifferences() {
        ROS_INFO("\n=== Difference Analysis ===");
        
        if (results_.size() < 2) return;
        
        ROS_INFO("Possible reasons for differences:");
        ROS_INFO("1. Different calibration methods and algorithms");
        ROS_INFO("2. Different number of calibration images");
        ROS_INFO("3. Different image quality and lighting conditions");
        ROS_INFO("4. Different corner detection algorithms");
        ROS_INFO("5. Different optimization criteria");
        ROS_INFO("6. Camera Info from topic may be factory calibration");
        ROS_INFO("7. ROS and Autoware may use different calibration patterns");
        
        // 分析具体差异
        double fx_diff = std::abs(results_[0].camera_matrix.at<double>(0, 0) - 
                                results_[1].camera_matrix.at<double>(0, 0));
        double fy_diff = std::abs(results_[0].camera_matrix.at<double>(1, 1) - 
                                results_[1].camera_matrix.at<double>(1, 1));
        
        if (fx_diff > 10.0 || fy_diff > 10.0) {
            ROS_WARN("Large differences detected in focal lengths!");
            ROS_WARN("This may indicate calibration quality issues.");
        } else {
            ROS_INFO("Differences are within acceptable range.");
        }
    }
    
    void saveComparisonResults() {
        std::ofstream fout(output_file_);
        
        fout << "=== Camera Calibration Comparison Analysis ===" << std::endl;
        fout << "Generated on: " << getCurrentTime() << std::endl << std::endl;
        
        for (size_t i = 0; i < results_.size(); i++) {
            fout << "Method: " << results_[i].method << std::endl;
            fout << "Camera Matrix:" << std::endl;
            fout << results_[i].camera_matrix << std::endl;
            fout << "Distortion Coefficients:" << std::endl;
            fout << results_[i].distortion_coeffs << std::endl;
            fout << "RMS Error: " << results_[i].rms_error << " pixels" << std::endl;
            fout << "Image Count: " << results_[i].image_count << std::endl;
            fout << "Image Size: " << results_[i].image_size.width << "x" << results_[i].image_size.height << std::endl;
            fout << "----------------------------------------" << std::endl;
        }
        
        fout.close();
        ROS_INFO("Comparison results saved to: %s", output_file_.c_str());
    }
    
    std::string getCurrentTime() {
        time_t now = time(0);
        char* dt = ctime(&now);
        return std::string(dt);
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "calibration_comparison");
    
    CalibrationComparison comparison;
    
    // 加载标定结果
    comparison.loadCalibrationResults();
    
    // 执行比较分析
    comparison.performComparison();
    
    ROS_INFO("Calibration comparison analysis completed");
    
    return 0;
}
